import os
import time
import cv2

from camera import Camera
from inference import RoadSegmentor
from utils.data_utils import apply_roi, calculate_road_position
from alert_system import AlertSystem


class RoadKeepSystem:
    def __init__(self):
        self.show_preview = os.getenv('SHOW_PREVIEW', '1') == '1'
        self.cam = Camera()
        self.seg = RoadSegmentor()
        self.alert = AlertSystem(min_interval=3.0)

    def start(self):
        cam_ok = self.cam.start()
        if not cam_ok:
            raise RuntimeError('摄像头启动失败')
        self.alert.start()

    def stop(self):
        self.cam.stop()
        self.alert.stop()

    def decide_status(self, ratio: float, offset: float):
        # ratio: 道路像素占比，过低则认为丢失；offset: -1~1，右偏为正
        if ratio < 0.08:
            return 'lost'
        if offset < -0.15:
            return 'left'
        if offset > 0.15:
            return 'right'
        return 'center'

    def draw_hud(self, img, ratio: float, offset: float, status: str):
        h, w = img.shape[:2]
        text = f"ratio={ratio:.2f} offset={offset:.2f} status={status}"
        cv2.rectangle(img, (10, 10), (10 + 330, 50), (0, 0, 0), -1)
        cv2.putText(img, text, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2, cv2.LINE_AA)
        # 中心线
        cv2.line(img, (w // 2, 0), (w // 2, h), (255, 255, 255), 1)

    def loop(self):
        try:
            while True:
                frame = self.cam.read()
                if frame is None:
                    time.sleep(0.01)
                    continue

                road_mask, overlay = self.seg.infer(frame, return_overlay=True)
                roi_mask = apply_roi(road_mask)
                ratio, offset = calculate_road_position(roi_mask)
                status = self.decide_status(ratio, offset)

                if status in ('left', 'right', 'lost'):
                    self.alert.enqueue(status)

                if self.show_preview:
                    vis = overlay if overlay is not None else frame.copy()
                    self.draw_hud(vis, ratio, offset, status)
                    cv2.imshow('RoadKeep', vis)
                    if cv2.waitKey(1) & 0xFF == 27:
                        break
        finally:
            self.stop()
            try:
                cv2.destroyAllWindows()
            except Exception:
                pass


def main():
    sys = RoadKeepSystem()
    sys.start()
    sys.loop()


if __name__ == '__main__':
    main()